
from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Remote, Motor, DCMotor
from pybricks.parameters import Button, Port, Stop, Color
from pybricks.tools import wait

class Remote_Controller:
    # Useful constant
    _PRESSED = 0
    _RELEASED = 1

    # Remote
    remote = None

    # Button status (bitwise operation)
    actual_buttons_status = 0
    previous_buttons_status = actual_buttons_status

    #  Buttons callback
    callback_center_pressed = None
    callback_center_released = None

    callback_left_center_pressed = None
    callback_left_center_released = None

    callback_left_plus_pressed = None
    callback_left_plus_released = None

    callback_left_minus_pressed = None
    callback_left_minus_released = None

    callback_right_center_pressed = None
    callback_right_center_released = None

    callback_right_plus_pressed = None
    callback_right_plus_released = None

    callback_right_minus_pressed = None
    callback_right_minus_released = None

    def __init__(self):
        self.remote = Remote()
        pressed = self.remote.buttons.pressed()
        
        # Check if buttons are pressed
        if Button.CENTER in pressed:
            self.actual_buttons_status += 1 

        if Button.LEFT in pressed:
            self.actual_buttons_status += 2

        if Button.LEFT_PLUS in pressed:
            self.actual_buttons_status += 4

        if Button.LEFT_MINUS in pressed:
            self.actual_buttons_status += 8

        if Button.RIGHT in pressed:
            self.actual_buttons_status += 16

        if Button.RIGHT_PLUS in pressed:
            self.actual_buttons_status += 32

        if Button.RIGHT_MINUS in pressed:
            self.actual_buttons_status += 64

        # Save old values
        self.previous_buttons_status = self.actual_buttons_status

    def register_callback(self, button, action, callback):
        if button == Button.CENTER:
            if action == self._PRESSED:
                self.callback_center_pressed = callback
            elif action == self._RELEASED:
                self.callback_center_released = callback

        if button == Button.LEFT:
            if action == self._PRESSED:
                self.callback_left_center_pressed = callback
            elif action == self._RELEASED:
                self.callback_left_center_released = callback

        if button == Button.LEFT_PLUS:
            if action == self._PRESSED:
                self.callback_left_plus_pressed = callback
            elif action == self._RELEASED:
                self.callback_left_plus_released = callback

        if button == Button.LEFT_MINUS:
            if action == self._PRESSED:
                self.callback_left_minus_pressed = callback
            elif action == self._RELEASED:
                self.callback_left_minus_released = callback

        if button == Button.RIGHT:
            if action == self._PRESSED:
                self.callback_right_center_pressed = callback
            elif action == self._RELEASED:
                self.callback_right_center_released = callback

        if button == Button.RIGHT_PLUS:
            if action == self._PRESSED:
                self.callback_right_plus_pressed = callback
            elif action == self._RELEASED:
                self.callback_right_plus_released = callback

        if button == Button.RIGHT_MINUS:
            if action == self._PRESSED:
                self.callback_right_minus_pressed = callback
            elif action == self._RELEASED:
                self.callback_right_minus_released = callback

    def update(self):
        # Update status
        pressed = self.remote.buttons.pressed()
        self.actual_buttons_status = 0

        # Check if buttons are pressed
        if Button.CENTER in pressed:
            self.actual_buttons_status += 1

        if Button.LEFT in pressed:
            self.actual_buttons_status += 2

        if Button.LEFT_PLUS in pressed:
            self.actual_buttons_status += 4

        if Button.LEFT_MINUS in pressed:
            self.actual_buttons_status += 8

        if Button.RIGHT in pressed:
            self.actual_buttons_status += 16

        if Button.RIGHT_PLUS in pressed:
            self.actual_buttons_status += 32

        if Button.RIGHT_MINUS in pressed:
            self.actual_buttons_status += 64

        # Callback
        if self.actual_buttons_status != self.previous_buttons_status:
            if (self.actual_buttons_status & 1) != (self.previous_buttons_status & 1):
                if self.actual_buttons_status & 1 != 0:
                    if self.callback_center_pressed != None:
                        self.callback_center_pressed()
                else:
                    if self.callback_center_released != None:
                        self.callback_center_released()

            if (self.actual_buttons_status & 2) != (self.previous_buttons_status & 2):
                if self.actual_buttons_status & 2 != 0:
                    if self.callback_left_center_pressed != None:
                        self.callback_left_center_pressed()
                else:
                    if self.callback_left_center_released != None:
                        self.callback_left_center_released()
                        
            if (self.actual_buttons_status & 4) != (self.previous_buttons_status & 4):
                if self.actual_buttons_status & 4 != 0:
                    if self.callback_left_plus_pressed != None:
                        self.callback_left_plus_pressed()
                else:
                    if self.callback_left_plus_released != None:
                        self.callback_left_plus_released()
                        
            if (self.actual_buttons_status & 8) != (self.previous_buttons_status & 8):
                if self.actual_buttons_status & 8 != 0:
                    if self.callback_left_minus_pressed != None:
                        self.callback_left_minus_pressed()
                else:
                    if self.callback_left_minus_released != None:
                        self.callback_left_minus_released()
                        
            if (self.actual_buttons_status & 16) != (self.previous_buttons_status & 16):
                if self.actual_buttons_status & 16 != 0:
                    if self.callback_right_center_pressed != None:
                        self.callback_right_center_pressed()
                else:
                    if self.callback_right_center_released != None:
                        self.callback_right_center_released()
                        
            if (self.actual_buttons_status & 32) != (self.previous_buttons_status & 32):
                if self.actual_buttons_status & 32 != 0:
                    if self.callback_right_plus_pressed != None:
                        self.callback_right_plus_pressed()
                else:
                    if self.callback_right_plus_released != None:
                        self.callback_right_plus_released()
                        
            if (self.actual_buttons_status & 64) != (self.previous_buttons_status & 64):
                if self.actual_buttons_status & 64 != 0:
                    if self.callback_right_minus_pressed != None:
                        self.callback_right_minus_pressed()
                else:
                    if self.callback_right_minus_released != None:
                        self.callback_right_minus_released()

        # Save old values
        self.previous_buttons_status = self.actual_buttons_status

def init_steering():
    global steering_motor
    global angle_right
    global angle_left
    global angle_zero

    angle_right = steering_motor.run_until_stalled(-200, duty_limit=75, then=Stop.HOLD)
    angle_left = steering_motor.run_until_stalled(200, duty_limit=75, then=Stop.HOLD)
    angle_zero = (angle_left + angle_right) / 2
    steering_motor.run_target(200, angle_zero, then=Stop.HOLD , wait=False)

def steer_left():
    global steering_motor
    global angle_left
    steering_motor.run_target(2000, angle_left + 5, then=Stop.HOLD, wait=False)

def steer_right():
    global steering_motor
    global angle_right
    steering_motor.run_target(2000, angle_right - 5, then=Stop.HOLD, wait=False)

def return_to_center():
    global steering_motor
    global angle_zero
    steering_motor.run_target(2000, angle_zero, then=Stop.HOLD, wait=False)

def press_power_plus():
    global direction
    global power
    global brake

    direction = 1
    power = 1

    #print("Press motor power plus ", direction, power, brake)
    set_drive_motor()

def press_power_minus():
    global direction
    global power
    global brake

    direction = -1
    power = 1

    #print("Press motor power minus ", direction, power, brake)
    set_drive_motor()
    
def release_power():
    global direction
    global power
    global brake

    direction = 0
    power = 0

    #print("Release power ", direction, power, brake)
    set_drive_motor()

def press_power_brake():
    global direction
    global power
    global brake

    brake = 1

    #print("Press power brake ", direction, power, brake)
    set_drive_motor()

def release_power_brake():
    global direction
    global power
    global brake

    brake = 0

    #print("Release power brake ", direction, power, brake)
    set_drive_motor()

def init_differential():
    global lock_motor
    global diff_is_locked

    lock_differential_motor.dc(50)
    wait(750)
    lock_differential_motor.stop()
    diff_is_locked = False

def lock_differential():
    global lock_motor
    global diff_is_locked
    global remote_controller
    global hub 
    
    if diff_is_locked == False:
        lock_differential_motor.dc(-50)
        wait(750)
        lock_differential_motor.stop()
        diff_is_locked = True
        remote_controller.remote.light.on(Color.RED)
        hub.light.on(Color.RED)
    else:
        lock_differential_motor.dc(50)
        wait(750)
        lock_differential_motor.stop()
        diff_is_locked = False
        remote_controller.remote.light.on(Color.GREEN)
        hub.light.on(Color.GREEN)

def set_drive_motor():
    global drive_motor_1
    global drive_motor_2
    global direction
    global power
    global brake

    if power == 0 and brake == 0:
        drive_motor_1.stop()
        drive_motor_2.stop()
    elif power == 0 and brake == 1:
        drive_motor_1.brake()
        drive_motor_2.brake()
    elif power > 0 and brake == 0:
        drive_motor_1.dc(direction * 100)
        drive_motor_2.dc(direction * 100)
    elif power > 0 and brake == 1:
        drive_motor_1.dc(direction * 50)
        drive_motor_2.dc(direction * 50)

# *** Main ***
print("Start main program")

# Init devices
hub = TechnicHub()
connect_flashing_time = [75, 75, 75, 75, 75, 1000]
hub.light.blink(Color.WHITE, connect_flashing_time)

steering_motor = Motor(Port.D)
[kp, kd, ki, maxi, ratei] = steering_motor.control.pid()
steering_motor.control.pid(7 * kp, 2 * kd, ki, 5 * maxi, 5 * ratei)
angle_right = 0
angle_left = 0
angle_zero = 0
init_steering()

drive_motor_1 = Motor(Port.A)
drive_motor_2 = Motor(Port.B)
direction = 0
power = 0
brake = 0

lock_differential_motor = DCMotor(Port.C)
diff_is_locked = False
init_differential()

print("Press center button on remote to connect...")
remote_controller = Remote_Controller()
remote_controller.remote.light.on(Color.GREEN)
hub.light.on(Color.GREEN)

remote_controller.register_callback(Button.LEFT, remote_controller._PRESSED, lock_differential)
remote_controller.register_callback(Button.LEFT_PLUS, remote_controller._PRESSED, steer_left)
remote_controller.register_callback(Button.LEFT_PLUS, remote_controller._RELEASED, return_to_center)
remote_controller.register_callback(Button.LEFT_MINUS, remote_controller._PRESSED, steer_right)
remote_controller.register_callback(Button.LEFT_MINUS, remote_controller._RELEASED, return_to_center)

remote_controller.register_callback(Button.RIGHT, remote_controller._PRESSED, press_power_brake)
remote_controller.register_callback(Button.RIGHT, remote_controller._RELEASED, release_power_brake)
remote_controller.register_callback(Button.RIGHT_PLUS, remote_controller._PRESSED, press_power_plus)
remote_controller.register_callback(Button.RIGHT_PLUS, remote_controller._RELEASED, release_power)
remote_controller.register_callback(Button.RIGHT_MINUS, remote_controller._PRESSED, press_power_minus)
remote_controller.register_callback(Button.RIGHT_MINUS, remote_controller._RELEASED, release_power)

print("Press any button and see effect")

while True:
    remote_controller.update()

 
